//如果连第一个锥桶都没见到的话会根据last随机取到的distance乱跑导致的
#pragma once
#include <fstream>
#include <iostream>
#include <cmath>
#include <opencv2/highgui.hpp>
#include <opencv2/opencv.hpp>
#include "../include/common.hpp"
#include "../include/detection.hpp" // Ai模型预测
#include "./track.cpp"

#define ALLOW 3
#define BLOCK_OUT 50
#define OUT_ALLOW 1000
#define SQUARE 16
#define ENABLE_CONE_VALUE (ROWSIMAGE/2)
#define ENABLE_BLOCK_VALUE (ROWSIMAGE*3/4)
#define EFFECCTIVE_DIFFERENCE (ROWSIMAGE/2)
#define LEFT (SQUARE*2)
#define RIGHT (SQUARE*2)
#define BLOCK_ADD (SQUARE*3)
class Danger
{
private:
    //炸弹检测标志
    int bomb=0;
    //状态标志
    enum STEP
    {
        None=0,
        Enable,
    };
    STEP step;
    //补线
    bool enter_l;
    bool enter_r;
    //存锥桶和障碍
    int predict_num;
    // PredictResult cone_block[5];
    // //最低的物体
    // PredictResult lowest;

    //包含左右上下 中间 标签的结构体

    int exit;
    bool block_flag;

    //上一次的障碍物
    PredictResult last;

    long startTime;
    long preTime;

//找到cone block;
    vector<PredictResult> get_cone_block(vector<PredictResult> predict)
    {
        vector<PredictResult> cone_block;
        for(int i=0;i<predict.size();i++)
        {
            if (predict[i].type == LABEL_CONE||predict[i].type==LABEL_BLOCK) // 锥桶检测
            {
//赛曙坐标系是左手系，yolo坐标系竖着是y横着是x
                cone_block.push_back(predict[i]);
            }
        }
        return cone_block;
    }
//找出其中最低的一个
    PredictResult get_lowest(vector<PredictResult> cone_block)
    {
        PredictResult lowest;
        lowest.y=0;
        lowest.height=0;
        lowest.type=LABEL_NONE;
        if(!cone_block.empty())
        {
            for(int i=0;i<cone_block.size();i++)
            {
                if(cone_block[i].y>lowest.y)
                {
                    lowest=cone_block[i];
                }
            }
        }
        return lowest;
    }
    
//找出目标靠左边线还是右边线（根据目标最低处是靠左边线近还是右边线近）（2.左边线=0右边线等于COLSIMAGE-1的时候）
//先不考虑极端状况
void judgement(track & track,PredictResult lowest)
{
    enter_l=false;
    enter_r=false;
    int down=lowest.y+lowest.height;
    //防止出界    
    int middle=lowest.x+(lowest.width)/2;
    int distance_l=abs(middle-track.l_border[down].y);
    int distance_r=abs(track.r_border[down].y-middle);
    if(distance_l<distance_r){
        enter_l=true;
        last=lowest;
    }else{
        enter_r=true;
        last=lowest;
    }
    return;
}
//根据目标靠左右边线；用他的位置来对相应的边线进行补线
void add_line(track & track,PredictResult lowest)
{
    if(lowest.type==LABEL_BLOCK)
    {
        if(enter_l==true)
        {
            for(int i=0;i<ROWSIMAGE;i++)
            {
                track.l_border[i].y=(lowest.x+(lowest.width)+BLOCK_ADD);//最右侧
            }
        }if(enter_r==true)
        {
            for(int i=0;i<ROWSIMAGE;i++)
            {
                track.r_border[i].y=(lowest.x-BLOCK_ADD);//最左侧
            }
        }
    }else{
        if(enter_l==true)
        {
            for(int i=0;i<ROWSIMAGE;i++)
            {
                track.l_border[i].y=(lowest.x+(lowest.width)+LEFT);//最右侧
            }
        }if(enter_r==true)
        {
            for(int i=0;i<ROWSIMAGE;i++)
            {
                track.r_border[i].y=(lowest.x-RIGHT);//最左侧
            }
        }
    }
}
public:
void init()
{
    last.height=0;
    last.width=0;
    last.x=0;
    last.y=0;
    last.type=LABEL_NONE;
}
//对两个锥桶一个障碍分三阶段？
//三个共用同一种处理？

//检测炸弹
    bool if_danger(vector<PredictResult> predict)
    {
        for(int i=0;i<predict.size();i++)
        {
            if (predict[i].type == LABEL_BOMB ) 
            {
                bomb++;
                break;
            }
        }
        if(bomb==ALLOW)
        {
            step=STEP::Enable;//下一次进入危险区的标志位
            bomb=0;
            
            block_flag=false;//推出标志位
            exit=0;

            last.type=LABEL_NONE;//上一次障碍物初始化
            return 1;
        }
        return 0;
    }


    //两锥桶一障碍
    //如果下半屏幕有目标则根据当前目标位置画线
    //如果当前屏幕没有目标进入下一阶段

    //没有目标则延续上一次画线的位置
    //直到检测到目标



//全60cm
//先假设车身完全可以回正，距离绝对够
//目标到一定位置的时候开始补线
//目标检测不到的时候停止补线
/*yolo预测结果不是左手系*/
int process(track &track,vector<PredictResult>predict)
{
    vector<PredictResult>cone_block=get_cone_block(predict);
    PredictResult lowest=get_lowest(cone_block);

//没检测到||检测到的目标在上半屏幕
    if(((lowest.y+lowest.height)>ENABLE_CONE_VALUE&&lowest.type==LABEL_CONE)||((lowest.y+lowest.height)>ENABLE_BLOCK_VALUE&&lowest.type==LABEL_BLOCK)){
        exit=0;
        judgement(track,lowest);//判断障碍物左右;更新last
        add_line(track,last);//补线
    }else if(lowest.type==LABEL_NONE)//下边线在value上面
    {
        //1.如果已经经过障碍物了,判断是否要退出
        if(last.type==LABEL_BLOCK)
        {
            if(exit<=BLOCK_OUT)
            {
                preTime=0;
                startTime=0;
            }
            add_line(track,last);
            exit++;
            if(exit==BLOCK_OUT)
            {
                preTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//程序开始时间
                startTime=preTime;
                return 0;
            }
            if(exit>BLOCK_OUT)
            {
                startTime = chrono::duration_cast<chrono::milliseconds>(chrono::system_clock::now().time_since_epoch()).count();//帧率计算
                if((startTime - preTime)>OUT_ALLOW)
                {return 1;}
            }
        }

        //2.1如果之前有检测到达标锥筒/障碍物,使用上一次的结果补线后退出
        if(last.type!=LABEL_NONE)
        {
            add_line(track,last);
            return 0;
        }
        //2.2没有,不做任何处理;
        else
        {return -1;}
    //在阈值以下
    }else
    {
        add_line(track,last);
        return 0;
    }

    return 0;
}
    cv::Scalar getCvcolor(int index)
    {
        switch (index)
        {
        case 0:
            return cv::Scalar(0, 255, 0); // 绿
            break;
        case 1:
            return cv::Scalar(255, 255, 0); // 天空蓝
            break;
        case 2:
            return cv::Scalar(0, 0, 255); // 大红
            break;
        case 3:
            return cv::Scalar(0, 250, 250); // 大黄
            break;
        case 4:
            return cv::Scalar(250, 0, 250); // 粉色
            break;
        case 5:
            return cv::Scalar(0, 102, 255); // 橙黄
            break;
        case 6:
            return cv::Scalar(255, 0, 0); // 深蓝
            break;
        case 7:
            return cv::Scalar(255, 255, 255); // 大白
            break;
        case 8:
            return cv::Scalar(247, 43, 113);
            break;
        case 9:
            return cv::Scalar(40, 241, 245);
            break;
        case 10:
            return cv::Scalar(237, 226, 19);
            break;
        case 11:
            return cv::Scalar(245, 117, 233);
            break;
        case 12:
            return cv::Scalar(55, 13, 19);
            break;
        case 13:
            return cv::Scalar(255, 255, 255);
            break;
        case 14:
            return cv::Scalar(237, 226, 19);
            break;
        case 15:
            return cv::Scalar(0, 255, 0);
            break;
        default:
            return cv::Scalar(255, 0, 0);
            break;
        }
    }

    void draw(Mat & binary_mat)
    {
        if(step!=STEP::None)
        {
            putText(binary_mat,"dangerScene:"+to_string(step) , Point(COLSIMAGE/3,15),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
            putText(binary_mat,"enter_l:"+to_string(enter_l) , Point(COLSIMAGE/3,30),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
            putText(binary_mat,"enter_r:"+to_string(enter_r) , Point(COLSIMAGE/3,45),FONT_HERSHEY_TRIPLEX, 0.3, Scalar(0, 0, 255), 1, CV_AA);
            //绘制last
            PredictResult result = last;
            auto score = std::to_string(result.score);
            int pointY = result.y - 20;
            if (pointY < 0)
                pointY = 0;
            //左上角 宽（向左）高（向下）
            cv::Rect rectText(result.x, pointY, result.width, 20);
            cv::rectangle(binary_mat, rectText, getCvcolor(result.type), -1);
            std::string label_name = result.label + " [last" + score.substr(0, score.find(".") + 3) + "]";
            cv::Rect rect(result.x, result.y, result.width, result.height);
            cv::rectangle(binary_mat, rect, getCvcolor(result.type), 1);
            cv::putText(binary_mat, label_name, Point(result.x, result.y), cv::FONT_HERSHEY_PLAIN, 1, cv::Scalar(0, 0, 254), 1);
        }
    }
    
};